#include <time.h>
#include <stdio.h>                                   
#include <stdlib.h>                                  
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <iostream>
#include <string.h>
#include <sstream>
#include <queue>
using namespace std;


#include "LookAheadAStarMD.h"

const int NO_GOAL_DEPTH = 1101;
const int GOAL_FOUND = 100;
bool immedateExpand = false;

// FOR DEBUG - REMOVE ME AFTER
FILE *debugFilePtr;
const bool PRINT_EXPANDED=false;
const bool PERFORM_TRIVIAL=true;
// END FOR DEBUG - REMOVE ME AFTER

// ------------------------------------------------
//                  Parameters
//-------------------------------------------------

//----------------- State Definition ---------------
struct operators                    
{
	int num;                                  /* number of applicable oprs: 2..4 */
	int pos[4];
} oprs[BOARDSIZE];     /* position of adjacent tiles for each position */


//---------- Hash table definitions (total memory) --------------
const long MAX = 20000000;                              /*max size of memory (hash table)*/
const long FULL = 19100000;                             /* max size of open list */
 struct hashtable                                 /* table of generated states */
{                            
	 short boardState[SIZE];
	 short g;                                      /* distance from initial state */
	 short h;                        /* static heuristic distance from goal state */
	 short oldblank;                                /* previous position of blank */
	 short currentblank;                            /* Current position of blank */ //-->not sure if needed???
	 int link;                    /* pointer to next element with same hash value */
	 int heaplink;
} old[MAX];      /* index of next state in open list, last is NIL */

 int bucket[MAX];   /* pointer to first element in table with given hash value */
 int empty;                      /* index of first free location in hash table */
 //unsigned int hashPowers[SIZE][SIZE]; /* supporting vector to calculate hash value*/

//------------- Heap definitions (total memory) ----------------
int heap[FULL];
int bestf;

int OpenStart;         /* index of first element of OPEN list*/
int OpenEnd;			/* index of last element of open list*/


//---------------- Heuristic definition ------------------------------
//unsigned char h1[h1_size];
//int tilePosForGetH_1[PATTERNSIZE_1+1];
//bool isBelongToPattern[SIZE]; /* boolean array - true for tokens included in PDB, false for tokens not included*/
//int locOfPatternInTiles[SIZE];
//int rotatedStateForGetH[SIZE];/*used to calculate hash and find goal*/
short manhat [BOARDSIZE][BOARDSIZE];  /* manhattan distance of tile in a given position */

//--------------- Counters -------------------------------------
int UpperLimit;                              /* current goal best cost */
int expanded;                               /* total number of nodes expanded */
int generated;                             /* total number of nodes generated */
int stored;                                  /* total number of states stored */
int ImmediateGenerated;                      /* total number of states generated by changing the parent (without allocating mameory)*/
int IE;                                     /* total number of states expanded and entered immediatlly to the close list*/
int reopened;                              /* total number of states that were re-entered from close to open*/
int reclosed;
int goalDepth;                             /* minimum number of steps from initial state to goal*/
int opened;                                /* total number of states entered to open list */
int closed;                                /* total number of states entered to close list */
int maxopen;                               /* maximum size of open list */
int copen;                                 /* supporting counter to determine maxopen*/
int duplicatCounter;                       /* number of states that already exist in memory*/
int CaseCounter;                           /* count the number of problems solved */
int incon;

double time2search;                        /* timer measuring search time for a single state-problem*/

/***************************************************************************************************/
// -------------------------------------------------
//                  Functions
//--------------------------------------------------


//------------- General utilities ------------------
void PrintResult2File(char* SearchName,int lookahead)
{
	FILE *f1;
		
	f1 = fopen("LH_100_15TilePuzzle_MD_recalc.csv","a");
	fprintf(f1,"%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f\n",SearchName,lookahead,CaseCounter,expanded,IE,stored,goalDepth,ImmediateGenerated,maxopen,opened,closed,duplicatCounter,reopened,reclosed,time2search);
	fclose(f1);
}

void printBoardState(FILE* pFilePointer, int stateIndex){
	for(int i=0;i<SIZE;i++){
		fprintf(pFilePointer, "%d",old[stateIndex].boardState[i]);
	}
}


//-------------- State (changing & updaing)---------
// Function name	: InitOps
// Description	    : This function state for each tile in the puzzle the allowed moves
// Return type		: void 
void InitOps ()

{
	int blank;                                   /* possible positions of blank */

	for (blank = 0; blank < BOARDSIZE; blank++)  /* for each possible blank position */
   {
	   oprs[blank].num = 0;                               /* no moves initially */
		if (blank > TPS - 1)                                       /* not top edge */
			oprs[blank].pos[oprs[blank].num++] = blank - TPS;       /* add a move up */
		if (blank % TPS > 0)                                      /* not left edge */
			oprs[blank].pos[oprs[blank].num++] = blank - 1;     /* add a move left */
		if (blank % TPS < TPS - 1)                                 /* not right edge */
			oprs[blank].pos[oprs[blank].num++] = blank + 1;    /* add a move right */
		if (blank < BOARDSIZE - TPS)                                 /* not bottom edge */
			oprs[blank].pos[oprs[blank].num++] = blank + TPS;   /* add a move down */
	} 
}

bool IsGoal(int stateIndex)
{
	for(int i=0;i<SIZE-1;i++)
		if(old[stateIndex].boardState[i]>old[stateIndex].boardState[i+1])
			return false;

	return true;
}


bool IsGoal(short state[BOARDSIZE])
{	
	for(int i=0;i<SIZE-1;i++)
		if(state[i]>state[i+1])
			return false;
	return true;
}



///-------------- heuristic (uploading and using)----------

// Function name	: InitManhattan
// Description	    : This function pre-computes the incremental evaluation function table. For a given tile
//                    in a given position, it calculates the Manhattan distance of the tile  
// Return type		: void

void InitManhattan (short manhat [BOARDSIZE][BOARDSIZE])
{int tile;                                                  /* number of tile */
 int position;                                            /* position of tile */

 for (tile = 1; tile < BOARDSIZE; tile++)                    /* all physical tiles */
   for (position = 0; position < BOARDSIZE; position++)     /* all legal positions */
     manhat [tile][position]  = abs((tile % TPS) - (position % TPS))   
                              + abs((tile / TPS) - (position / TPS));
}

// Function name	: CalcManhattan
// Description	    : returns the sum of the Manhattan distances of each tile, except the blank, from its goal position 
// Return type		: int
int CalcManhattan (short s[BOARDSIZE])
{int value;                                                    /* accumulator */
 int pos;                                                    /* tile position */

  value = 0;
  for (pos = 0; pos < BOARDSIZE; pos++)
    if (s[pos] != 0)             /* blank isn't counted in Manhattan distance */
      value = value + manhat[s[pos]][pos]; /* manhattan distance of that tile */
  return(value);}


//----- Heap activities (hash, insert, delete) --------
void Heapify_up(int iloc)
{
	int parent;
	int parentg;
	int parentf;
	int child;
	int childg;
	int childf;

	parent=floor((double)(iloc-1)/2);
	parentg=old[heap[parent]].g;
	parentf=parentg+old[heap[parent]].h;

	child=iloc;
	childg=old[heap[child]].g;
	childf=old[heap[child]].h+childg;

	if(parent<0) return;
	if(parentf>childf || (parentf==childf && childg>=parentg)) // Tie brake in favor of higher g (lower h)
	{
		//printf("%s\n","switching");
		int temp=heap[parent];
		old[heap[parent]].heaplink=child;
		old[heap[child]].heaplink=parent;
		heap[parent]=heap[child];
		heap[child]=temp;
		
		Heapify_up(parent);
	}

}

void Heapify(int iloc)
{
	int right;
	int left;

	int rightf;
	int rightg;

	int leftf;
	int leftg;

	int itemf;
	int itemg;

	int minCost=iloc;

	right=2*iloc+2;
	left=2*iloc+1;


	itemf=old[heap[iloc]].g+old[heap[iloc]].h;
	itemg=old[heap[iloc]].g;

	if(right<= OpenStart)
	{
		rightf=old[heap[right]].g+old[heap[right]].h;
		rightg=old[heap[right]].g;
		
	}
	if (left<=OpenStart)
	{
		leftf=old[heap[left]].g+old[heap[left]].h;
		leftg=old[heap[left]].g;

	}


	if(left<=OpenStart && (leftf<itemf || (itemf==leftf && leftg>=itemg)))
	{
		minCost=left;
	}
	else
	{
		minCost=iloc;
	}

	if(right<=OpenStart && 
		(rightf<old[heap[minCost]].h+old[heap[minCost]].g || 
			(old[heap[minCost]].h+old[heap[minCost]].g==rightf && rightg>=old[heap[minCost]].g))) // Tie brake in favor of high g values (low h)
	{
		minCost=right;
	}

	if(minCost!=iloc)
	{
		old[heap[iloc]].heaplink=minCost;
		old[heap[minCost]].heaplink=iloc;
		int temp=heap[iloc];
		heap[iloc]=heap[minCost];
		heap[minCost]=temp;
		Heapify(minCost);
	}
}



//----- Hash activities (hash, insert, delete) --------

unsigned int CalcHash (short ps[BOARDSIZE])

//                                      /* packed state of puzzle */
 
{int index;                                              /* tile being hashed */
 unsigned int value;
                                                     /* hash value */

 unsigned int TempArray[WORDS];
 int word;                                    /* current word in packed state */
 int pos;                               /* current position in expanded state */
 

 for (word = 0; word < WORDS; word++) /* initialize packed state to all zeros */
   TempArray[word] = 0; 

 for (pos = 0; pos < BOARDSIZE; pos++)               /* for each position in state */
   {
	   word = pos /TPW;                         /* current word of packed state */
	   TempArray[word] = TempArray[word] * BOARDSIZE + ps[pos];/* add tile in base BOARDSIZE */
	}
 value = TempArray[0];                                             /* starting value */
 for (index = 1; index < WORDS; index++)          /* for each additional tile */
   value = value * 3 + TempArray[index];                   /* add new tile in base 3 */

 return(value%MAX);
}         /* returned value will be mod 2 ^ word size (MAX - size of hash array)*/


int SearchHash (short state2search[SIZE],unsigned int hashval)
{
	int index;                        /* index to different states in hash table */
	int pos;                            /* index to different positions in state */

	index = bucket[hashval];                     /* first state in target bucket */

	if(hashval<0)
	{
		printf("%s %d %s %d\n","index=",index,"bucket[ -7483648]=",bucket[-7483648] );
		string mystr;
		getline (cin,mystr);
	}
	while (index != MAX)                         /* end of chain not reached yet */
	{
		if(index<0) printf ("%s\n","in while");
		for (pos = 0; pos < SIZE; pos++)           /* each position in the state */
			if (state2search[pos] != old[index].boardState[pos]) 
				break;    /* mismatch: exit for loop */
		if (pos == SIZE) /* state found, return location */
			return (index);         
		else /* next state on chain */
			index = old[index].link;                     
	}

	return (-1);
}



void InsertHash (int parentNode,unsigned int hashval, int parentBlank,int tileChange, int g, int h)

{
	//int word;                                            /* word in packed state */
	 int f;                                            /* combined merit function - no need - BFS*/
	 int index;                           /* index in hash table where state goes */

	 index = empty++;           /* store in first empty location , empty defined globaly */
	 stored++;                        /* increment number of states in hash table */
	 opened++;
	 copen++;

	 if(maxopen<copen) maxopen=copen;

	for (int i=0;i<SIZE;i++)
	{
		old[index].boardState[i]=old[parentNode].boardState[i];
	}
	old[index].oldblank=parentBlank;
	old[index].currentblank = tileChange;
	

	//3. update h & g
	old[index].g=g;
	old[index].h=h;
	 
	

	 //TODO - need to re-think the assigment of the hash list per hashvalue (instead of adding the last node first, add it to the end of the list)
	 old[index].link = bucket[hashval];      /* next pointer to old first element */
	 bucket[hashval] = index;               /* new state is at head of hash chain */
	 f = old[index].g + old[index].h;


	//Heap OPEN list		
	OpenStart++;

	heap[OpenStart]=index;
	old[index].heaplink=OpenStart;
	Heapify_up(OpenStart);

    if (f < bestf) bestf = f;

}

void DeleteFromOpenList (int index)
{
	int f;                                     /* F value of state to be deleted */

	f = old[index].g + old[index].h;       /* compute F value of state */
	closed++;
	copen--;
	////DELETE from HEAP	
	int ploc=old[index].heaplink;//the parent location in the heap array
	old[index].heaplink=-1;
	//printf("%s %d\n","ploc=",ploc);
	if(ploc>OpenStart || ploc<0)
	{
		//printf("%s\n","No such element");

	}
	else
	{
		heap[ploc]=heap[OpenStart];
		old[heap[OpenStart]].heaplink=ploc;
		OpenStart--;
		Heapify(ploc);

	}

}

/**
Adds a node to the closed list, that has not entered OPEN. This occurs when trivial lookahead is applied.
**/
void Add2Close(int parentNode,unsigned int hashval, int parentBlank,int tileChange, int g, int h)
{
	int index;
	
	index = empty++;           /* store in first empty location , empty defined globaly */
	stored++; 
	closed++;
	IE++;

	for (int i=0;i<SIZE;i++)
	{
		old[index].boardState[i]=old[parentNode].boardState[i];
	}
	old[index].oldblank=parentBlank;
	old[index].currentblank = tileChange;

	//3. update h & g
	old[index].g=g;
	old[index].h=h;


	 //TODO - need to re-think the assigment of the hash list per hashvalue (instead of adding the last node first, add it to the end of the list)
	 old[index].link = bucket[hashval];      /* next pointer to old first element */
	 bucket[hashval] = index;               /* new state is at head of hash chain */
	 //old[index].prev=MAX;

}
void ReOpen(int stateIndex, int newg, int parentBlank,int TileChange)
{
	int ReArIndex=-1;

	old[stateIndex].g=newg;
	old[stateIndex].oldblank=parentBlank;
	old[stateIndex].currentblank = TileChange;
	ReArIndex=old[stateIndex].heaplink;

	if (old[stateIndex].heaplink==-1)
	{
		opened++;
		copen++;
		closed--;

		if(maxopen<copen) maxopen=copen;

		OpenStart++;
		ReArIndex=OpenStart;
		heap[OpenStart]=stateIndex;
		old[stateIndex].heaplink=OpenStart;
	}
	Heapify_up(ReArIndex);
}



//-------------- Search activities --------------------

int ExpandNode(int stateIndex,int g,int h, int TileChange, int oldblank)
{
	int blank;
	int newblank;
	int newindex;
	int opindex;

	int newg;
	int newh;
	int newf;
	int tile;
	int flag=-1;

	unsigned int hashval;



	blank=TileChange;
	for (opindex = 0; opindex < oprs[blank].num; opindex++)
	{
	 if ((newblank = oprs[blank].pos[opindex]) != oldblank)
	 {
		 //calculate the cost of the new child
		 tile=old[stateIndex].boardState[newblank];
		 newh=h+manhat[tile][blank] - manhat[tile][newblank];
		 newg=g+1;
		 newf=newh+newg;
		 
		 //Perform the switch
		 old[stateIndex].boardState[blank]=tile;
		 old[stateIndex].boardState[newblank]=0;

				//check if the changed node is a goal
				if(newh==0)
				{

					//		short test1[SIZE]={2,3,4,5,6,7,8,9,10,11,12,13,14,15,0,1};
					//bool testa=true;
					//for(int i=0;i<SIZE;i++)
					//		if(old[stateIndex].boardState[i]!=test1[i]) testa=false;

					//if(testa)
					//{
					//	flag=0;
					//	printf("%s %d\n","goal is found in depth: ",newf);
					//}
					//TODO-debug printing
					printf("%s\n","found a goal");
					//printf("%s %d %s %d\n","f=",newf," g=",newg);
					//printf("%s %d %s %d\n","parent h=",old[stateIndex].h, " parent g=",old[stateIndex].g); 
					//printf("%s","The goal state is: ");
					//for (int b=0;b<SIZE;b++)
					//	printf("%d ",old[stateIndex].boardState[b]);
					//printf("%s\n","");


					//UpperLimit=newf;
					//goalDepth=newf;
					//flag=0;
					//for (int i=0;i<SIZE;i++)
					//	if(old[stateIndex].boardState[i]!=test[i]) flag=-1;
					//string mystr;
					//getline (cin,mystr);
				}

				//Calc hash and check if already exists
				hashval=CalcHash(old[stateIndex].boardState);
				newindex=SearchHash(old[stateIndex].boardState,hashval);
				if(newindex==stateIndex) newindex=-1;

				//If the child does not exist, add to the OPEN list
				if(newindex==-1)
				{

					//if(flag==0)
					//{
					//	printf("%s %d\n","Adding to index:",empty+1);
					//	flag=-1;
					//	string mystr;
					//     getline (cin,mystr);
					//}
					InsertHash(stateIndex,hashval,blank,newblank,newg,newh);

				}
				 else //if the node already exists
				 {
					 duplicatCounter++;
					 // If there's a duplicate node with higher g (the h are the same==>f is higher)
					  if (old[newindex].g>newg)
						 {
							 //printf("%s %d %s %d %s %d\n","current g= ",old[newindex].g," better g= ",newg," newf=",newf);
							 //printf("%s %d\n","heaplink=", old[newindex].heaplink);
 							//string mystr;
							 //getline (cin,mystr);
							 reopened++;// the node is in the close list
							ReOpen(newindex, newg, blank,newblank);
							//DeleteFromOpenList(newindex);
							//old[newindex].g=newg;
							//old[newindex].h=newh;
							//InsertHash(stateIndex,hashval,newblank,newg,newh);
						 }

				}
				old[stateIndex].boardState[newblank]=tile;
				old[stateIndex].boardState[blank]=0;
	 }

				
		
	}//end for
		return 0;
}//end procedure


int LimitedLookAhead(int stateIndex,int g, int h,int lookahead, int TileChange, int oldblank)
{
	int blank;                                      /* current position of blank */
	int opindex;                                    /* index into operator array */
	int newblank;                                 /* blank position in new state */
	int tile;                                                /* tile being moved */
	int newg;
	int newh;
	int newf;
	int minF = INT_MAX;

	blank=TileChange;


	//printf("%s\n","in Limited lookahead");
	for (opindex = 0; opindex < oprs[blank].num; opindex++)
	{
		if ((newblank = oprs[blank].pos[opindex]) != oldblank)
		{
			tile=old[stateIndex].boardState[newblank];

			// Perform pseudo move
			old[stateIndex].boardState[blank]=tile;
			old[stateIndex].boardState[newblank]=0;
			ImmediateGenerated++;

			newh=h+manhat[tile][blank] - manhat[tile][newblank];
			//newh=eval78(stateIndex,blank, newblank);

			newg=g+1;
			newf=newh+newg;

			//if the f-value of the potential child is lower than the current lookahead - create it
			if(newf<=lookahead && newf<UpperLimit)
			{
				if(newh==0 && IsGoal(stateIndex))
				{
					if (newf<minF) 
						minF=newf; // Min. cost to udpate root with

					if (newf<UpperLimit)
					{					
						goalDepth=newf;
						UpperLimit=newf;
						//return 100; // Assumes uniform edge cost
					}	
				}
				else
				{
					newf=LimitedLookAhead(stateIndex,newg,newh,lookahead,newblank,blank);
					if (newf<minF) 
						minF=newf;
				}	
			}
			else{
				if (newf<minF) minF=newf; // Min. cost to udpate root with
			}

			// Undo pseudo move
			old[stateIndex].boardState[newblank]=tile;
			old[stateIndex].boardState[blank]=0;
		}
	}//end big for
	return minF; 
}


/**
Expansion cycle of simple A*
**/
int SimpleExpandNode(int stateIndex,int g,int h, int TileChange, int oldblank)
{
	// REMOVE AFTER DEBUG
	if(PRINT_EXPANDED){
		fprintf(debugFilePtr,"Expanded, %d,",stateIndex);
		printBoardState(debugFilePtr,stateIndex);			
		fprintf(debugFilePtr,", %d,%d\n",g,h);
	}	// END REMOVE AFTER DEBUG
	int blank;
	int newblank;
	int newindex;
	int opindex;

	int newg;
	int newh;
	int newf;
	int tile;
	int flag=-1;
	unsigned int hashval;
	expanded++;	

	blank=TileChange;
	for (opindex = 0; opindex < oprs[blank].num; opindex++)
	{
		newblank = oprs[blank].pos[opindex];
		if (newblank != oldblank)
		{
			//calculate the cost of the new child
			tile=old[stateIndex].boardState[newblank];
			newh=h+manhat[tile][blank] - manhat[tile][newblank];
			//newh=MAX(newh,h-1); // Pathmax Rule 1
			newg=g+1;
			newf=newh+newg;
			
			// Change parent
			old[stateIndex].boardState[blank]=tile;
			old[stateIndex].boardState[newblank]=0;

			//Calc hash and check if already exists
			hashval=CalcHash(old[stateIndex].boardState);
			newindex=SearchHash(old[stateIndex].boardState,hashval);
			if(newindex==stateIndex){ // Happens at initialization, not sure why (will check later)
				newindex=-1;
			}

			// If generated node is not new - reinsert to OPEN (reopen if closed and found better way - occurs only when h is inconsistent)
			if(newindex!=-1) 
			{
				duplicatCounter++;
				if (old[newindex].g>newg) // If found a faster way to reach that node
				{
					flag=2;
					if(old[newindex].heaplink==-1) 
						reclosed++;
					else 
						reopened++;
					ReOpen(newindex, newg, blank,newblank);
				}
			}
			else // The generated node is new - insert to OPEN
			{
				InsertHash(stateIndex,hashval,blank,newblank,newg,newh);
			}
			
			// Undo parent
			old[stateIndex].boardState[newblank]=tile;
			old[stateIndex].boardState[blank]=0;
		}	
	}//end for
	return 0;
}//end procedure


int const NODE_TO_INSERT = 0;
int const NODE_TO_UPDATE = 2;
int const EMPTY_FLAG = -1;
int IIExpandNode(int stateIndex,int g,int h,int lookahead, int TileChange, int oldblank)
{
	// REMOVE AFTER DEBUG
	if(PRINT_EXPANDED){
		fprintf(debugFilePtr,"Expanded, %d,",stateIndex);
		printBoardState(debugFilePtr,stateIndex);			
		fprintf(debugFilePtr,", %d,%d\n",g,h);
	}
	// END REMOVE AFTER DEBUG

	int blank;
	int newblank;
	int newindex;
	int opindex;
	int fInLookahead;
	int newg;
	int newh;
	int newf;
	int tile;
	int flag=-1;
	bool immediateExpand;
	unsigned int hashval;
	expanded++;

	blank=TileChange;
	for (opindex = 0; opindex < oprs[blank].num; opindex++)
	{
		immediateExpand=false;
		newblank = oprs[blank].pos[opindex];
		if (newblank != oldblank)
		{
			//calculate the cost of the new child
			tile=old[stateIndex].boardState[newblank];
			newh=h+manhat[tile][blank] - manhat[tile][newblank];
			//newh=MAX(newh,h-1); // Pathmax Rule 1
			newg=g+1;
			newf=newh+newg;

			if(newf<UpperLimit){	// Ignore nodes with cost higher than the current goal upper bound				
				// Change parent
				old[stateIndex].boardState[blank]=tile;
				old[stateIndex].boardState[newblank]=0;

				//check if the changed node is a goal
				if(newh==0 && IsGoal(stateIndex))
				{
					if(newf<UpperLimit)
					{
						UpperLimit=newf;
						goalDepth=newf;
						return GOAL_FOUND;
					}
				}

				//Calc hash and check if already exists
				hashval=CalcHash(old[stateIndex].boardState);
				newindex=SearchHash(old[stateIndex].boardState,hashval);
				if(newindex==stateIndex){ // Happens at initialization, not sure why (will check later)
					newindex=-1;
				}
				// ------------------   Trivial Lookahead ----------------------------
				//if the current child has cost which is equal or less than the root cost -> add it to the close list and continue developing its children
				if((newf<=g+h)&&(PERFORM_TRIVIAL))
				{
					if(newindex==-1) // If child is new, immediatly expand him
					{
						Add2Close(stateIndex, hashval, blank, newblank,newg,newh);
						immediateExpand=true;
					}//end if newindex
					else // If child already in OPEN or CLOSED
					{					
						duplicatCounter++;
						if (old[newindex].g>newg) // If newg is better than old - need to remove from OPEN and rexpand (NOTE: IGNORED NODE REOPENING FOR INCONSISTENT H)
						{
							if(old[newindex].heaplink==-1)
								{closed--;reclosed++;}
							else 
								{IE++;reopened++;}

							DeleteFromOpenList(newindex);
							old[newindex].g=newg;
							old[newindex].h=newh;							
							immedateExpand=true;
						}
					}
					if(immedateExpand=true){ // If need to immediately expand
						if (IIExpandNode(stateIndex,newg,newh,lookahead,newblank,blank)==GOAL_FOUND)
							return GOAL_FOUND;
					}
				}
				else{
					flag = EMPTY_FLAG;
					// --------------------------------- Lookahead if necessary ---------------------------
					if(newindex!=-1) // If node is not new, maybe need to update cost or reopen
					{
						duplicatCounter++;
						if (old[newindex].g>newg)
						{
							flag=NODE_TO_UPDATE;
						}
					}
					else
					{
						flag=NODE_TO_INSERT;
					}
					//If this is a new node or a re-entered node - perform a lookahead
					if((flag==NODE_TO_INSERT) || (flag==NODE_TO_UPDATE))
					{
						if(lookahead>0)
						{
							fInLookahead=LimitedLookAhead(stateIndex,newg,newh,old[stateIndex].g+old[stateIndex].h+lookahead,newblank,blank); 
							//newh = MAX(newh,fInLookahead-newg); // Update newh with new h value found in lookahead (min. of all descendandts)
						}

						//Only if this is a new node - insert to the open list
						if(flag==NODE_TO_INSERT)
							InsertHash(stateIndex,hashval,blank,newblank,newg,newh);	
						else if (flag==NODE_TO_UPDATE){
							old[newindex].h=newh;
							//old[newindex].g=newg; Done inside ReOpen()
							if(old[newindex].heaplink==-1) 
								reclosed++;
							else 
								reopened++;
							ReOpen(newindex, newg, blank,newblank);
						}
					}
				}
				// Undo parent
				old[stateIndex].boardState[newblank]=tile;
				old[stateIndex].boardState[blank]=0;
			}// end if f less than UpperBound
		}// end if legal move
	}//end for
	return 0;
}//end procedure


/** 
Sets up global variables for an A* based search
**/
void setupSearch(){
	
	InitOps();
	InitManhattan(manhat);
	//Init Hash table
	for (int i = 0; i< MAX; i++)                    /* empty hash table */
    {
		old[i].g = -1;                  /* only g value checked for emptiness */
		old[i].heaplink=-1;
		bucket[i] = MAX;
	}

	for (int t=0;t<FULL;t++)
	{
		heap[t]=-1;
	}

	//Init Open List pointers
	OpenStart=0; // beginning of the open list
	OpenEnd = 0; //End of the open list

	//Init next free loaction index in hash table
	empty=0;

	//Init counters
	incon=0;
	maxopen=0;
	copen=0;
	expanded=0;                               
	generated=0;                             
	stored=0;
	reopened=0;
	reclosed=0;
	opened=0;
	closed=0;
	ImmediateGenerated=0;
	IE=0;
	UpperLimit=NO_GOAL_DEPTH; //TODO - need to find logical upper limit
	duplicatCounter=0;
	goalDepth=0;
}

void AStarWithLookahead (short s[SIZE], int lookahead)
{
	//Variables
	int blank;
	unsigned int CurrentHashValue;
	int CurrentDepth=0; //Init goal test inidicator
	int CurrentOpenNode=0; //next node in the open list to check
	double startclock;
	double endclock;
	UpperLimit = INT_MAX;
	
	setupSearch();

	//Find the blank position
	for (blank=0;blank<SIZE;blank++)
		if (s[blank]==0) break;

	//Start counting time
	startclock = (double)clock();
	CurrentHashValue=CalcHash(s);
	//Insert the initial state to the hash table and update the open list
	for (int word = 0;word<SIZE;word++)
		old[empty].boardState[word]=s[word];
	old[empty].oldblank = blank;
	old[empty].currentblank = blank;
	old[empty].g = 0;
	old[empty].h=bestf=CalcManhattan(old[empty].boardState);
	//old[empty].h=bestf=eval78(empty,blank,blank);
	old[empty].link = bucket[CurrentHashValue];      /* next pointer to old first element */
	old[empty].heaplink=0;
	 bucket[CurrentHashValue] = empty;               /* new state is at head of hash chain */
	int f = old[empty].g + old[empty].h; 
	empty++;
	heap[0]=0;
	//
	int currentLimit=0;

	// First iteration performes a full lookahead 
	if (lookahead>0) 
		LimitedLookAhead(0,old[heap[0]].g,old[heap[0]].h,
			old[heap[0]].g+old[heap[0]].h+lookahead,
			old[heap[0]].currentblank,old[heap[0]].oldblank);
	
	//Searching loop:
	//While the goal was not found and hash table is not full and the current f value is lower than the upper limit
	while(CurrentDepth!=GOAL_FOUND && stored <FULL && 
		old[heap[0]].h+old[heap[0]].g<UpperLimit && 
		OpenStart>=0)
	{
		bestf=old[heap[0]].h+old[heap[0]].g;
		CurrentOpenNode=heap[0];

		//If the current node is a goal - stop the loop
		//if (old[CurrentOpenNode].h==0 && IsGoal(CurrentOpenNode))
		//{
		//	if (DEBUG_PRINTS) 
		//		printf("%s\n","--->>> Found goal!<<< ---");
		//	CurrentDepth=100;
		//	goalDepth=old[CurrentOpenNode].g;
		//	UpperLimit=goalDepth; // DEBUG - RONI REMOVE ME!
		//}
		if(UpperLimit<NO_GOAL_DEPTH)
		{
			if (DEBUG_PRINTS) 
				printf("%s\n","--->>> Found goal!<<< ---");
			CurrentDepth=GOAL_FOUND;
			goalDepth=UpperLimit;			
		}
		//Else (meaning, this is a node which is not a goal and with f value lower than upper limit - need to develop it
		else
		{			
			DeleteFromOpenList(CurrentOpenNode); //delete the node expanded from the open list		
			CurrentDepth=IIExpandNode(CurrentOpenNode,old[CurrentOpenNode].g,
				old[CurrentOpenNode].h,
				lookahead,
				old[CurrentOpenNode].currentblank,
				old[CurrentOpenNode].oldblank);	

			if ((expanded % 10000000)==0)
				printf("Expanded %d [%d]\n",expanded, bestf);
		}
	}
	//if(IsGoal(heap[0])) goalDepth=old[heap[0]].g;
	goalDepth=UpperLimit;
	endclock = (double)clock();
	time2search = (endclock-startclock)/(double)CLOCKS_PER_SEC;
	PrintResult2File("AL*",lookahead);
	printf("%s","expanded= ");
	printf("%d \n",expanded);
	printf("%s","IE= ");
	printf("%d \n",IE);
	printf("%s","stored = ");
	printf("%d \n",stored);
	printf("%s","reopened = ");
	printf("%d \n",reopened);
	printf("%s","IG= ");
	printf("%d \n",ImmediateGenerated);
	printf("%s","opened = ");
	printf("%d \n",opened);
	printf("%s","closed = ");
	printf("%d \n",closed);
	printf("%s %d\n","maxopen = ",maxopen);
	printf("%s","goalDepth = ");
	printf("%d \n",goalDepth);
	printf("%s", "Start Time = ");
	printf ("%f \n",startclock);
	printf("%s", "Stop Time = ");
	printf ("%f \n",endclock);
	printf ("%s","Duplicated = ");
	printf ("%d \n",duplicatCounter);
	printf ("%s","incon = ");
	printf ("%d \n",incon);
	printf("%s", "time2search = ");
	printf ("%f \n",time2search);
}

/**
A simple A* search. No lookahead, no inconsistency and no BPMX.
**/
void SimpleClassicAStar (short s[SIZE])
{
	//Variables
	int blank;
	unsigned int CurrentHashValue;
	int CurrentDepth=0; //Init goal test inidicator
	int CurrentOpenNode=0; //next node in the open list to check
	double startclock;
	double endclock;

	setupSearch();

	//Find the blank position
	for (blank=0;blank<SIZE;blank++)
		if (s[blank]==0) break;

	//Start counting time
	startclock = (double)clock();
	CurrentHashValue=CalcHash(s);
	//Insert the initial state to the hash table and update the open list
	for (int word = 0;word<SIZE;word++)
		old[empty].boardState[word]=s[word];
	old[empty].oldblank = blank;
	old[empty].currentblank = blank;
	old[empty].g = 0;
	old[empty].h=bestf=CalcManhattan(old[empty].boardState);
	//old[empty].h=bestf=eval78(empty,blank,blank);
	old[empty].link = bucket[CurrentHashValue];      /* next pointer to old first element */
	old[empty].heaplink=0;
	bucket[CurrentHashValue] = empty;               /* new state is at head of hash chain */
	int f = old[empty].g + old[empty].h; 
	empty++;
	heap[0]=0;
	int currentLimit=0;

	//Searching loop:
	//While the goal was not found and hash table is not full and the current f value is lower than the upper limit
	while(CurrentDepth!=100 && stored <FULL && OpenStart>=0)
	{
		bestf=old[heap[0]].h+old[heap[0]].g;
		CurrentOpenNode=heap[0];

		//If the current node is a goal - stop the loop
		if (old[CurrentOpenNode].h==0 && IsGoal(CurrentOpenNode))
		{
			if (DEBUG_PRINTS) 
				printf("%s\n","--->>> Found goal!<<< ---");
			CurrentDepth=100;
			goalDepth=old[CurrentOpenNode].g;
		}
		//Else (meaning, this is a node which is not a goal and with f value lower than upper limit - need to develop it
		else
		{			
			DeleteFromOpenList(CurrentOpenNode); //delete the node expanded from the open list		
			SimpleExpandNode(CurrentOpenNode,old[CurrentOpenNode].g,
				old[CurrentOpenNode].h,
				old[CurrentOpenNode].currentblank,
				old[CurrentOpenNode].oldblank);
			
			if ((expanded % 10000000)==0)
				printf("Expanded %d [%d]\n",expanded, bestf);
		}
	}
	endclock = (double)clock();
	time2search = (endclock-startclock)/(double)CLOCKS_PER_SEC;
	PrintResult2File("A*",0);
	printf("%s","expanded= ");
	printf("%d \n",expanded);
	printf("%s","IE= ");
	printf("%d \n",IE);
	printf("%s","stored = ");
	printf("%d \n",stored);
	printf("%s","reopened = ");
	printf("%d \n",reopened);
	printf("%s","IG= ");
	printf("%d \n",ImmediateGenerated);
	printf("%s","opened = ");
	printf("%d \n",opened);
	printf("%s","closed = ");
	printf("%d \n",closed);
	printf("%s %d\n","maxopen = ",maxopen);
	printf("%s","goalDepth = ");
	printf("%d \n",goalDepth);
	printf("%s", "Start Time = ");
	printf ("%f \n",startclock);
	printf("%s", "Stop Time = ");
	printf ("%f \n",endclock);
	printf ("%s","Duplicated = ");
	printf ("%d \n",duplicatCounter);
	printf ("%s","incon = ");
	printf ("%d \n",incon);
	printf("%s", "time2search = ");
	printf ("%f \n",time2search);
}



/**
A single iteration of IDA*
state - The current state
g - the g of state
h - the h of state
blank - the position of the blank in state
oldblank - the previous position of the blank in state (used to prune small transpositions)

Returns the next threshold (the minimum f value found that exceeded the current threshold
**/
int IDAStarIteration(short state[SIZE],int g, int h, int threshold,int blank, int oldblank){
	int newblank;
	int tile;
	int newh;
	int newg;
	int newf;
	int minNextF = INT_MAX;


	for (int opindex = 0; opindex < oprs[blank].num; opindex++)
	{
		newblank = oprs[blank].pos[opindex];
		if (newblank != oldblank)
		{
			++generated;

			//calculate the cost of the new child
			tile=state[newblank];
			newh=h+manhat[tile][blank] - manhat[tile][newblank];
			//newh=MAX(newh,h-1); // Pathmax Rule 1
			newg=g+1;
			newf=newh+newg;
			if (newf<UpperLimit){ // Only if f is under the cost of the previously found goal there is a point in continuing the search
				// Move tile
				state[blank]=tile;
				state[newblank]=0;
				
				if((newh==0) && (IsGoal(state))){
					if(UpperLimit>newg){
						UpperLimit=newg;
					}
					// Undo tile movement
					state[newblank]=tile;
					state[blank]=0;
					return newg;
				}
				else{
					if(newf<=threshold){
						newf = IDAStarIteration(state,newg,newh,threshold,newblank,blank);
						if(newf<minNextF) // Update min next f (possibly setting the new threshold)
							minNextF=newf;
						++expanded;
					}
					else{ // Update min next f (possibly setting the new threshold)
						if (minNextF>newf)
							minNextF=newf;
					}
				}
				// Undo tile movement
				state[newblank]=tile;
				state[blank]=0;
			}// End if(newf<UpperLimit)

		}	
	}//end for
	return minNextF;
}

/**
A simple IDA* search. No lookahead, no inconsistency and no BPMX.
**/
void IDAStar (short state[SIZE])
{
	//Variables
	int blank;
	int h;
	int g;
	double startclock;
	double endclock;
	int threshold;
	setupSearch();

	//Find the blank position
	for (blank=0;blank<SIZE;blank++)
		if (state[blank]==0) break;

	//Start counting time
	startclock = (double)clock();
	h = CalcManhattan(state);
	g=0;
	threshold = h+g;
	UpperLimit=INT_MAX;
	//Searching loop:
	while(threshold<UpperLimit){		
		threshold=IDAStarIteration(state,g,h,threshold,blank,blank);
		printf("Threshold %d, Expanded %d\n",threshold,expanded);
	}
	goalDepth=UpperLimit;
	endclock = (double)clock();
	time2search = (endclock-startclock)/(double)CLOCKS_PER_SEC;
	PrintResult2File("IDA*",0);
	printf("%s","expanded= ");
	printf("%d \n",expanded);
	printf("%s","IE= ");
	printf("%d \n",IE);
	printf("%s","stored = ");
	printf("%d \n",stored);
	printf("%s","reopened = ");
	printf("%d \n",reopened);
	printf("%s","IG= ");
	printf("%d \n",ImmediateGenerated);
	printf("%s","opened = ");
	printf("%d \n",opened);
	printf("%s","closed = ");
	printf("%d \n",closed);
	printf("%s %d\n","maxopen = ",maxopen);
	printf("%s","goalDepth = ");
	printf("%d \n",goalDepth);
	printf("%s", "Start Time = ");
	printf ("%f \n",startclock);
	printf("%s", "Stop Time = ");
	printf ("%f \n",endclock);
	printf ("%s","Duplicated = ");
	printf ("%d \n",duplicatCounter);
	printf ("%s","incon = ");
	printf ("%d \n",incon);
	printf("%s", "time2search = ");
	printf ("%f \n",time2search);
}


/**
Sets a struct tm object with the current time
**/
char* getLocalTime(){
	time_t currentTime=time(NULL);
	return asctime(localtime(&currentTime));
}



//-------------- MAIN -------------------------------
void main(int argc, char *argv[])
{
	short s703[BOARDSIZE]={9,1,0,6,2,13,7,3,4,10,11,15,8,12,14,5};//30
	short s800[BOARDSIZE]={9,6,0,15,2,1,13,7,4,10,3,11,8,12,14,5};//40
	short s1000[BOARDSIZE]={4,3,14,13,9,1,5,10,0,15,7,12,11,6,8,2};//52
	FILE *states_ptr;
	InitOps();
	InitManhattan(manhat);
	int oldGoalDepth=-1;
	//Init counters
	CaseCounter=0;
	printf(ProblemsFile);
	states_ptr=fopen(ProblemsFile,"r");
    if (states_ptr==NULL){printf ("ProblemsFile not found or File can not be opened\n"); exit(0);}
	//Run search



	//ClassicAStar(s703,2);
	//ClassicAStar(s800,2);
	//ClassicAStar(s1000,2);
	short tempState[SIZE];
	while(!feof(states_ptr) && CaseCounter<150)
	{
		CaseCounter++;	
		fscanf(states_ptr,"%*d"); 
		for(int i=0;i<SIZE;i++)
			fscanf(states_ptr,"%d",&tempState[i]);
		oldGoalDepth=FULL;
		if(CaseCounter==13)//CaseCounter<101)
		{
			printf("%s\n","-------------------------------------------");
			for(int k=0;k<SIZE;k++)
					printf("%d ",tempState[k]);
			printf("%s\n","");

			printf("%s----- IDA*[%d] ----- \n",getLocalTime(),CaseCounter);
			IDAStar(tempState);
			oldGoalDepth=goalDepth;

			printf("%s----- Classic A*[%d] ----- \n",getLocalTime(),CaseCounter);

			// FOR DEBUG - REMOVE ME AFTER
			if(PRINT_EXPANDED)
				debugFilePtr = fopen("c:\\temp\\testClassic.txt","w");
			// END FOR DEBUG - REMOVE ME AFTER
			SimpleClassicAStar(tempState);
			// FOR DEBUG - REMOVE ME AFTER
			if(PRINT_EXPANDED)
				fclose(debugFilePtr);
			// END FOR DEBUG - REMOVE ME AFTER


			for(int i=0;i<11;i=i+2){
				// FOR DEBUG - REMOVE ME AFTER
				if(PRINT_EXPANDED)
					debugFilePtr = fopen("c:\\temp\\testLH0.txt","w");
				// END FOR DEBUG - REMOVE ME AFTER



				printf("%s----- Classic AL*(%d)[%d] ----- \n",getLocalTime(),i,CaseCounter);
				AStarWithLookahead(tempState,i);
				// FOR DEBUG - REMOVE ME AFTER
				if(PRINT_EXPANDED)
					fclose(debugFilePtr);
				break;
				// END FOR DEBUG - REMOVE ME AFTER
					


			}
		}
	}

	// FOR DEBUG - REMOVE ME AFTER
	if(PRINT_EXPANDED)
		fclose(debugFilePtr);
	// END FOR DEBUG - REMOVE ME AFTER

	printf("%s\n","Done!!");    
}
